/*
 * Created on Feb 12, 2007
 *
 * TODO To change the template for this generated file go to
 * Window - Preferences - Java - Code Style - Code Templates
 */
package org.pricejd.rcx.unstable;

import josx.platform.rcx.LCD;
import josx.platform.rcx.Motor;
import josx.platform.rcx.Sensor;
import josx.platform.rcx.SensorConstants;
import josx.platform.rcx.SensorListener;
import josx.platform.rcx.Sound;

/**
 * The LightNavigator class contains methods for performing basic navigational
 * movements.  This class uses an encoder wheel attached to a 40 tooth gear
 * to act as the "poor man's rotation sensor".  Alot of this code is adabted from
 * RotationNavigator which comes with Lejos.
 * 
 * @author Jon Price
 * @see josx.robotics.RotationNavigator
 */
public class LightNavigator implements SensorConstants{
	// These values may need to be configured.
	private final int whiteValue = 50;
	private final int blackValue = 40;
	private float COUNTS_PER_CM = .8f;
	private float COUNTS_PER_DEGREE_GEAR = .15f;
	private float COUNTS_PER_DEGREE_NOGEAR = .1f;
	
	
	// Motors for steering
	private Motor left;
	private Motor right;
	
	// Rotation Sensor
	private Sensor rotRight;
	private Sensor bumper;
	
	// Movement values
	private int lightCount;
	
	
	// Internal states
	private boolean moving;
	private boolean canMove;
	private boolean countWhite;
	private boolean countBlack;
	
	/**
	 * Allocates a LightNavigator object and initializes it with the
	 * proper motors and sensors.  The x and y values will each equal
	 * 0 (cm's) on initialization, and the starting angle is 0 degrees,
	 * so, if the first move is forward() the robot will run along the
	 * x axix.
	 * 
	 * @param rightMotor The motor used to drive the right wheel e.g. Motor.C.
	 * @param leftMotor The motor used to drive the left wheel e.g. Motor.A.
	 * @param rightRot The light sensor that is counting the rotation on the right.
	 * @param sBumper The bumper sensor
	 */
	public LightNavigator(Motor rightMotor, Motor leftMotor, Sensor rightRot, Sensor sBumper) {
		this.right = rightMotor;
		this.left = leftMotor;
		
		this.rotRight = rightRot;
		this.bumper = sBumper;
		this.rotRight.setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_PCT);
		
		moving = false;
		canMove = true;
		
		rightRot.addSensorListener(new SensorListener() {
			public void stateChanged(Sensor arg0, int arg1, int arg2) {
				if(arg2 < blackValue && countBlack) {
					lightCount++;
					countBlack = false;
					countWhite = true;
				}
				if(arg2 > whiteValue && countWhite) {
					lightCount++;
					countBlack = true;
					countWhite = false;
				}
			}
		});
		
		sBumper.addSensorListener(new SensorListener() {
			public void stateChanged(Sensor arg0, int arg1, int arg2) {
				left.stop();
				right.stop();
				Sound.beepSequence();
				canMove = false;
			}
		});
	}

	/**
	 * Rotates the the RCX a specific number of degrees in a direction
	 * (+ or -).  This method will return once the rotation is complete.
	 * 
	 * @param angle Angle to rotate in degrees.  A positive calue rotates
	 * 		left, a negative value rotates right.
	 */
	public void rotate(float angle) {
		// Calculate the number of intervals of rotation sensor to count
		int count = 0;
		
		// Turn on light sensor.
		rotRight.activate();
		countBlack = Sensor.S1.readValue() > whiteValue;
		countWhite = Sensor.S1.readValue() < blackValue;
		if(angle > 0) {
			count = (int)(COUNTS_PER_DEGREE_NOGEAR * angle);
			// Start motors.
			right.forward();
			left.backward();
			
		} else if (angle < 0) {
			count = (int)(COUNTS_PER_DEGREE_GEAR * angle);
			// Start motors.
			right.backward();
			left.forward();
		}
		moveRCX(Math.abs(count));
		
		// Turn off light sensor and stop motors.
		rotRight.passivate();
	}
	
	/**
	 * Moves the RCX a specific distance.  A positive value moves it forward
	 * and a negative value moves it backwards.
	 * 
	 * @param dist The positive or negative distnace to move (in cm's).
	 * @throws InterruptedException
	 */
	public void travel(int dist) {
		int count = (int)(dist * COUNTS_PER_CM);
		
		rotRight.activate();
		countBlack = Sensor.S1.readValue() > whiteValue;
		countWhite = Sensor.S1.readValue() < blackValue;
		if(dist > 0) {
			right.forward();
			left.forward();
		} else if (dist < 0) {
			right.backward();
			left.backward();
		}
		
		moveRCX(Math.abs(count));
		rotRight.passivate();
	}
	
	/**
	 * Moves the RCX for a specified number of counts on the
	 * light/rotation sensor.
	 * 
	 * Expects that the motors are moving.
	 */
	private void moveRCX(int count) {
		moving = true;
		while(moving) {
			if(lightCount > count) {
				left.stop();
				right.stop();
				moving = false;
			}
		}
		lightCount = 0;
		try {
			Thread.sleep(300);
		} catch (InterruptedException e) {
		}
	}
	
	public boolean getCanMove() {
		return canMove;
	}
}